
#include "ISimplexConvexCast.h"
#include "IDetectorResult.h"
#include "member_BShape.h"

ISimplexConvexCast::ISimplexConvexCast(const BShape* convexA, const BShape* convexB, IVoronoiSimplexSolver* simplexSolver)
	: _simplexSolver(simplexSolver),
	  m_convexA(convexA),
	  m_convexB(convexB)
{
}


bool ISimplexConvexCast::calcTimeOfImpact(const BMatrix& sourceA, const BMatrix& targetA, const BMatrix& sourceB, const BMatrix& targetB, CastResult& caseResult)
{
	_simplexSolver->reset();

	BVector linVelA = targetA.position() - sourceA.position();
	BVector linVelB = targetB.position() - sourceB.position();

	BReal lambda = 0;

	BMatrix interpMatrixA = sourceA;
	BMatrix interpMatrixB = sourceB;

	BVector relvel = (linVelA - linVelB);

	BVector supVertexA = sourceA * (shape_member(m_convexA)->supportVertex(sourceA.mult3(-relvel)));
	BVector supVertexB = sourceB * (shape_member(m_convexB)->supportVertex(sourceB.mult3(relvel)));
	BVector vector = supVertexA - supVertexB;

	BVector normal;

	BReal dist2 = vector.length2();

	BReal castEpsilon = REAL_EPSILON * 10;
	int maxIter = 64;
	while ((dist2 > castEpsilon) && maxIter--)
	{
		supVertexA = interpMatrixA * (shape_member(m_convexA)->supportVertex(interpMatrixA.mult3(-vector)));
		supVertexB = interpMatrixB * (shape_member(m_convexB)->supportVertex(interpMatrixB.mult3(vector)));
		BVector diffv = supVertexA - supVertexB;
		BReal VdotW = vector.dot(diffv);

		if (lambda > 1)
			return false;

		if (VdotW > 0)
		{
			BReal VdotR = vector.dot(relvel);
			if (VdotR >= -(REAL_EPSILON * REAL_EPSILON))
				return false;
			else
			{
				lambda = lambda - VdotW / VdotR;
				interpMatrixA.setPosition((1 - lambda) * sourceA.position() + lambda * targetA.position());
				interpMatrixB.setPosition((1 - lambda) * sourceB.position() + lambda * targetB.position());
				diffv = supVertexA - supVertexB;
				normal = vector;
			}
		}

		if (!_simplexSolver->inSimplex(diffv))
			_simplexSolver->addVertex(diffv, supVertexA, supVertexB);

		if (_simplexSolver->closest(vector))
		{
			dist2 = vector.length2();
		}
		else
		{
			dist2 = 0;
		}
	}

	caseResult.fraction = lambda;
	if (normal.length2() > REAL_EPSILON)
		caseResult.normal = normal.normal();
	else
		caseResult.normal.reset();

	if (caseResult.normal.dot(relvel) >= 0)
		return false;

	BVector hitA, hitB;
	_simplexSolver->compute_points(hitA, hitB);
	caseResult.point = hitB;
	return true;
}
